#include "main_tracker.h"
#include "local_sign.hpp"
#include "multiple_sign_tracker.hpp"
#include <base/fileUtils.h>
#include <sstream>

using namespace TRACK;
using namespace std;

MainTracker::MainTracker(const std::string& root_path) : root_path_(root_path), debug_message_(false)
{
	rp_MST = new MST(true);  
}

MainTracker::~MainTracker()
{
    delete(rp_MST);
	rp_MST = nullptr;
}

void MainTracker::Tracking(const DlFrame& dlFrame, 
	const hdmap_frame& hdmap_f, 
	const GeoPoint& gps,
	const Camera &cam,
	const Pose& pose,
	DbgFrame* dbg_frame
	)
{
    Camera_config camera_config(cam.get_K(), 
		cam.get_D(), 0.0, 0.0, 0.0, 0.0, cv::Size(cam.width, cam.height));

    std::vector<LOCAL_SIGN> signs;
	for (size_t index = 0; index < dlFrame.signs.size(); ++index)
    {
		LOCAL_SIGN sign;
		sign.id_ = index;
		sign.category_ = dlFrame.signs[index].category;
		sign.imagename_ = gps.imagename;
		sign.score_ = 0.0;

        Eigen::Vector2d min, max;
        dlFrame.signs[index].bBox.GetRange(min, max);
        sign.bbox_ = cv::Rect(cv::Point(min.x(), min.y()), cv::Point(max.x(), max.y()));
		sign.init(camera_config); 
		signs.push_back(sign);
	}

	cv::Mat src = dbg_frame->GetTopMat();
	rp_MST->run(signs, src);

	if(debug_message_)
		rp_MST->draw_result(src);
}

bool MainTracker::TakeFirst(SignBatch& to_Vec)
{
	if(!rp_MST->shaped_lists_.empty())
	{
		std::vector<LOCAL_SIGN> signs = rp_MST->shaped_lists_.back();
		rp_MST->shaped_lists_.pop_back();

		std::vector<SignVecMeta> signVecs;
		for(LOCAL_SIGN sign : signs)
		{
			DlSign dlSign;
			dlSign.bBox = envelope2D(sign.bbox_.x, sign.bbox_.y, sign.bbox_.x + sign.bbox_.width, sign.bbox_.y + sign.bbox_.height);
			dlSign.id = sign.id_;
			SignVecMeta meta(sign.imagename_, dlSign);
			signVecs.push_back(meta);
		}
		static int id = 0;
		to_Vec.id = ++id;
		to_Vec.SignVecs = signVecs;
		return true;
	}
	return false;


	/*if(!stable_lists_.empty())
	{
		Tracker tracker = stable_lists_.back();
		stable_lists_.pop_back();

		to_Vec.SignVecs.swap(tracker.sign_batches);
		to_Vec.id = tracker.id;

#if 0
		std::stringstream save_path;
		save_path << "./vectorization/" << tracker.id;
		FileUtils::folder_create(save_path.str());

		for (SignVecMeta sign_meta : to_Vec)
		{
			std::string image_file = FileUtils::file_concate(root_path_, sign_meta.imagename);
			cv::Mat origin_mat = cv::imread(image_file);
			cv::rectangle(origin_mat, sign_meta.dlSign.bBox, cv::Scalar(255, 255, 255), 1);
			std::string save_file = save_path.str() + "/" + sign_meta.imagename;
			cv::imwrite(save_file, origin_mat);
		}
#endif

		return true;
	}
	return false;*/
	return false;
}